#include "../core/mv_camera_tools.hpp"

#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>


#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>

#include "../core/mv_pub.hpp"

using namespace std;

// 根据传入的变形曲线数据校正图片
int MvCameraTools::cameraCalibration(Mat &src, InputArray curve, OutputArray o) {

	Mat curvePara = curve.getMat();
	Mat dst = o.getMat();

	int w = src.size().width;
	int h = src.size().height;

	int srcStep = src.step;
	uchar* srcData = src.data;

	for (int j = 0; j < h; j++) {
		uchar* proLine = dst.data + dst.step * j;

		for (int i = 0; i < w; i++) {
			float r = sqrt(j * j + i * i);
			int ir = (int) r;

			Point2f xy(0.f, 0.f);
			Point2f ip = curvePara.at<Point2f>(ir, 0);
			Point2f ipn = curvePara.at<Point2f>(ir + 1, 0);

			xy.x = ip.x * (ir - r);
			xy.x += (ipn.x * (1 + ir - r));
			xy.y = ip.y * (ir - r);
			xy.y += (ipn.y * (1 + ir - r));

			float x = xy.x;
			float y = xy.y;
			int ix = (int) xy.x;
			int iy = (int) xy.y;

			// 4个点合成1个点
			float p00, p01, p10, p11, pd;

			p00 = srcData[iy * srcStep + 0] * (x - ix);
			p01 = srcData[iy * srcStep + 3 + 0] * (1 + ix - x);
			pd = (p00 + p01) * (y - iy);
			p10 = srcData[(iy + 1) * srcStep + 0] * (x - ix);
			p11 = srcData[(iy + 1) * srcStep + 3 + 0] * (1 + ix - x);
			pd += (p10 + p11) * (1 - y + iy);
			proLine[i * 3 + 0] = pd + 0.5; // 目标点数据R

			p00 = srcData[iy * srcStep + 1] * (x - ix);
			p01 = srcData[iy * srcStep + 3 + 1] * (1 + ix - x);
			pd = (p00 + p01) * (y - iy);
			p10 = srcData[(iy + 1) * srcStep + 1] * (x - ix);
			p11 = srcData[(iy + 1) * srcStep + 3 + 1] * (1 + ix - x);
			pd += (p10 + p11) * (1 - y + iy);
			proLine[i * 3 + 1] = pd + 0.5; // 目标点数据G

			p00 = srcData[iy * srcStep + 2] * (x - ix);
			p01 = srcData[iy * srcStep + 3 + 2] * (1 + ix - x);
			pd = (p00 + p01) * (y - iy);
			p10 = srcData[(iy + 1) * srcStep + 2] * (x - ix);
			p11 = srcData[(iy + 1) * srcStep + 3 + 2] * (1 + ix - x);
			pd += (p10 + p11) * (1 - y + iy);
			proLine[i * 3 + 2] = pd + 0.5; // 目标点数据B
		}
	}

	return 0;
}
